#include "Chassis_Task.h"

// 缓慢加速减速控制
struct MotorSlowCtrl X_ACC_SLOW = {0, 80, 3}; // 绝对坐标系下的XY坐标轴下的缓慢加速
struct MotorSlowCtrl Y_ACC_SLOW = {0, 80, 3}; // 绝对坐标系下的XY坐标轴下的缓慢加速

extern osEventFlagsId_t xProcessEventGroupHandle;
extern osMessageQueueId_t ChassisMessageHandle; // 接收上层Process_Task的控制指令

// 底盘私密参数开发者不可更改
struct Chassis_Data {
    float Current_X, Current_Y;
};

static struct Chassis_Data g_chassis_data = {0,0}; // 这个别动就行！！！ Don't Change this!!!
static pidw_t pid_Zparam = {W_PID_MODE, Z_Kp, 0, Z_Kd, 0, 0, 0, 0, 0, {0, 0}, Z_OUTMIN, Z_OUTMAX, Z_ERRMIN, Z_ERRMAX, 0};
static pidw_t pid_Wparam = {W_PID_MODE, W_Kp, W_Ki, W_Kd, 0, 0, 0, 0, 0, {0, 0}, W_OUTMIN, W_OUTMAX, W_ERRMIN, W_ERRMAX, 0};
/* 底盘数据 */
Chassis_TypeDef Chassis = {
	0,0,0,0,0,0,0,
	&pid_Wparam,
	0,0,0,0,0,0,0,0,
	&pid_Zparam,
	Chassis_Static_Mode,
	0,
	&g_chassis_data,
};

static void Chassis_Init(void);
static void Get_Chassis_Info(void);
static void hwt101_init(void);
static void Chassis_Motor_Init(void);
static void Get_Motor_Speed(void);
static void Set_Motor_Speed(struct DCMOTOR_Device *pDev[],float v1, float v2, float v3, float v4);
static void Get_Car_XY_Speed(void);
static void Get_XY_Speed_And_Coord(void);
static void Get_XY_Coord(float *save); /* 获取小车当前位置 */
static void Set_XY_And_Angle(void);
static void Switch_Chassis_State(void); /* 状态切换 */
static void Get_Chassis_Ctrl_Data_From_Up(void);
/**
* @brief Function implementing the ChassisTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_Chassis_Task */
void Chassis_Task(void *argument)
{
  /* USER CODE BEGIN Chassis_Task */
	Chassis_Init(); // 底盘外设初始化
	#if Car_Param_DeBug // vofa参数调试时开启
	struct UART_Device *pUART1Dev_vofa;
	pUART1Dev_vofa = vofa_init("stm32_dma_uart1");
	#endif
	osDelay(1500);
	/*************************************************/
	TickType_t xLastWakeTime;
	const TickType_t xDelay5ms = pdMS_TO_TICKS( 5 );
	xLastWakeTime = xTaskGetTickCount();
  /* Infinite loop */
  for(;;) 
  {	
		/* 接收来自Process_Task的信息 eg：(tx, ty)/Chassis_Mode/Chassis_Angle */
		Get_Chassis_Ctrl_Data_From_Up();
		Switch_Chassis_State();
		Get_Chassis_Info(); // 获取车体信息
		Set_XY_And_Angle(); // 设置位置移动
		#if Car_Param_DeBug
		Send_fData_ToVofa(pUART1Dev_vofa, Chassis.pDCMoto[0]->v, Chassis.pDCMoto[1]->v, Chassis.pDCMoto[2]->v, Chassis.pDCMoto[3]->v);
		#endif
		LED_flash(CHASSIS_TASK_LED);
		vTaskDelayUntil( &xLastWakeTime, xDelay5ms );
  }
  /* USER CODE END Chassis_Task */
}

/******************************应用层***********************************/
/* 初始化 */
static void Chassis_Init(void)
{
	Chassis.f_StateChange = 0;
	Chassis.ChassisMode = Chassis_Static_Mode; // 底盘定点状态
	RK3588_Init();
	hwt101_init();
	Chassis_Motor_Init();
	while(1)
	{
		if (hwt101.Flag_UART_RX_OK)
		{
			hwt101.Flag_UART_RX_OK = 0;
			osDelay(1000);
			break;
		}
		osDelay(100);
	}
}
/* 获取小车底盘信息 */ 
static void Get_Chassis_Info(void)
{
	uint8_t Flag1 = 0, Flag2 = 0;

	Flag1 = hwt101.Flag_UART_RX_OK;
	hwt101.Flag_UART_RX_OK = 0;
	Flag2 = rk3588.Flag_UART_RX_OK;
	rk3588.Flag_UART_RX_OK = 0;
	
	if (Flag1)
	{
		Chassis.Z_angle = hwt101.taskBuffer[0];
	}
	if (Chassis.ChassisMode == Chassis_Vision_Location_Mode)
	{ // 在找圆/物料模式下
		if (Flag2)
		{
			Chassis.Kind = rk3588.taskBuffer[0];
			Chassis.imagex = rk3588.taskBuffer[1];
			Chassis.imagey = rk3588.taskBuffer[2];
		}
	}
	
	Get_Motor_Speed(); // 测量rpm
	// 
	Get_Car_XY_Speed();
	// 
	Get_XY_Speed_And_Coord();
	// 给PID计算赋反馈值
	Chassis.pid_Zparam->feedback_value = Chassis.Z_angle;
	for (int i = 0; i < 4; i++)
	{
		Chassis.pDCMoto[i]->pid_param->feedback_value = Chassis.pDCMoto[i]->v;
	}
}
/* 底盘控制主要函数 */
#define Slow_K 	35.f
float K = 1.0f; // 补偿
float error0 = 0; // 位置环误差
float error1 = 0;
// 视觉修正
float errx = 0, erry = 0;
#define CenterX		320 // 摄像头中心
#define CenterY		320
// 缓加减速
float Vtarget = 0;
uint16_t SlowT = 0;
/***************************************************/
static void Set_XY_And_Angle(void)
{
	float saveXY[2], alpha, v1 = 0, v2 = 0, v3 = 0, v4 = 0;
	
	/**********************/
	// 读取私密数据 ->小车位置坐标
	Get_XY_Coord(saveXY);// 读取当前坐标
	
	// 根据状态选择不同控制策略
	switch(Chassis.ChassisMode)
	{
		case Chassis_Static_Mode: // 锁住小车底盘
			Chassis.pid_Zparam->kp = Z_Kp_Static;
		  Chassis.pid_Zparam->kd = Z_Kd_Static;
			Chassis.pid_Wparam->out = 0;
			Set_wtraget(Chassis.pid_Zparam, Chassis.Tangle); // 设置目标值
			pid_angle(Chassis.pid_Zparam, 360.f); // 角度环PID
			break;
		case Chassis_Move_Mode: // 定点测试
			Chassis.pid_Zparam->kp = Z_Kp; 
		  Chassis.pid_Zparam->kd = Z_Kd; 
			arm_sqrt_f32((Chassis.TX - saveXY[0]) * (Chassis.TX - saveXY[0]) + (Chassis.TY - saveXY[1]) * (Chassis.TY - saveXY[1]), &error0); // 获取目标距离
			alpha = atan2(Chassis.TY - saveXY[1], Chassis.TX - saveXY[0]); // 获取直线运动角度(自然坐标系下)
			Chassis.pid_Wparam->out = Chassis.pid_Wparam->kp * error0 + Chassis.pid_Wparam->kd * (error0 - error1);
			error1 = error0;
			
			// 限幅
			if (Chassis.pid_Wparam->out >= W_OUTMAX)
			{
				Chassis.pid_Wparam->out = W_OUTMAX;
			}
			if (Chassis.pid_Wparam->out <= W_OUTMIN)
			{
				Chassis.pid_Wparam->out = W_OUTMIN;
			}
			//
			if (fabs(error0)< W_ERRMAX)
			{
				error0 = 0;
				Vtarget = 0;
				Chassis.ChassisMode = Chassis_Static_Mode; // 状态切换
			}
			Set_wtraget(Chassis.pid_Zparam, Chassis.Tangle); // 设置目标值
			pid_angle(Chassis.pid_Zparam, 360.f); // 角度环PID
			break;
		case Chassis_Turn_Mode: // 旋转
			Chassis.pid_Wparam->out = 0;
			Set_wtraget(Chassis.pid_Zparam, Chassis.Tangle); // 设置目标值
			pid_angle(Chassis.pid_Zparam, 360.f); // 角度环PID
			if (_fabsf(Chassis.pid_Zparam->err[0]) <= Chassis.pid_Zparam->Err_max)
			{
				Chassis.pid_Zparam->err[0] =0;
				Chassis.ChassisMode = Chassis_Static_Mode; // 状态切换
			}
			break;
		case Chassis_Vision_Location_Mode: // 找圆环/物料 使用非线性控制
			errx = Chassis.imagex - CenterX, erry = Chassis.imagey - CenterY;
			if (_fabsf(errx) <  F_ERRMIN && _fabsf(erry) < F_ERRMIN)
			{
				errx = 0, erry = 0;
				Chassis.ChassisMode = Chassis_Static_Mode;
			}
			Chassis.T_VX = erry * F_Kp;
			Chassis.T_VY = - errx * F_Kp;
			pid_angle(Chassis.pid_Zparam, 360.f); // 角度环PID
	}
	if (Chassis.ChassisMode == Chassis_Move_Mode)
	{
		
		float Vref = 0;
		float out = 0;
		Vref = Chassis.pid_Wparam->out;
		if (SlowT++ == 10)
		{
			SlowT = 0;
			float feedbackv;
			arm_sqrt_f32(Chassis.VX * Chassis.VX + Chassis.VY * Chassis.VY , &feedbackv);
			if (feedbackv < Vref && Vtarget <= Vref)
			{
				Vtarget += 10;
			}
			else if (feedbackv > Vref)
			{
				Vtarget -= 20;
			}
			if (Vtarget > Vref) {
				out = Vref;
			}
			else {
				out = Vtarget;
			}
			Chassis.T_VX = out * arm_cos_f32(alpha); // 速度分解到自然坐标系下
			Chassis.T_VY = out * arm_sin_f32(alpha);		
		}
	}
	else 
	{
		Chassis.T_VX = Chassis.pid_Wparam->out * arm_cos_f32(alpha); // 速度分解到自然坐标系下
		Chassis.T_VY = Chassis.pid_Wparam->out * arm_sin_f32(alpha);
	}
	/*****************************不动********************************************/	
	float ax;
	ax =  Chassis.Z_angle / 57.3f;
	Chassis.T_Vx = - Chassis.T_VY * arm_sin_f32(ax) + Chassis.T_VX * arm_cos_f32(ax); // 速度分解到车体坐标系下
	Chassis.T_Vy =  Chassis.T_VY * arm_cos_f32(ax) + Chassis.T_VX * arm_sin_f32(ax);	
	// 速度分解设置
	v1 = Chassis.T_Vy + Chassis.T_Vx + Chassis.pid_Zparam->out * CAR * K;
	v2 = Chassis.T_Vy - Chassis.T_Vx - Chassis.pid_Zparam->out * CAR * K;
 	v3 = Chassis.T_Vy + Chassis.T_Vx - Chassis.pid_Zparam->out * CAR * K;
 	v4 = Chassis.T_Vy - Chassis.T_Vx + Chassis.pid_Zparam->out * CAR * K;
	Set_Motor_Speed(Chassis.pDCMoto, v1, v2, v3, v4);
}
/* 获取小车当前位置 */
static void Get_XY_Coord(float *save)
{
	struct Chassis_Data *pData = Chassis.priv_data;
	save[0] = pData->Current_X;
	save[1] = pData->Current_Y;
}

/******************************底层***********************************/
/* 底盘电机初始化 */
static void Chassis_Motor_Init(void)
{
	Chassis.pDCMoto[0] = GetDCMOTORDevice("DC_Motor_one");
	Chassis.pDCMoto[1] = GetDCMOTORDevice("DC_Motor_two");
	Chassis.pDCMoto[2] = GetDCMOTORDevice("DC_Motor_three");
	Chassis.pDCMoto[3] = GetDCMOTORDevice("DC_Motor_four");
	//
	for (int i = 0;i < 4;i++)
		Chassis.pDCMoto[i]->Init(Chassis.pDCMoto[i]);
}
/* get rpm */
static void Get_Motor_Speed(void)
{
	for (int i = 0;i < 4;i++)
		Chassis.pDCMoto[i]->GetVelocity(Chassis.pDCMoto[i]);
}
/* rpm */
static void Set_Motor_Speed(struct DCMOTOR_Device *pDev[],float v1, float v2, float v3, float v4)
{
	float v_target[4] = {v1, v2, v3, v4};
	for (int i = 0;i < 4;i++)
		Chassis.pDCMoto[i]->SetVelocity(Chassis.pDCMoto[i], v_target[i]);
}
/* 获取车体坐标系下速度 */
#define CK  1.0f
static void Get_Car_XY_Speed(void)
{
	Chassis.Vx = (Chassis.pDCMoto[2]->v + Chassis.pDCMoto[0]->v - Chassis.pDCMoto[1]->v - Chassis.pDCMoto[3]->v) / 4 * CK * 0.13f; // cm/s
	Chassis.Vy = (Chassis.pDCMoto[3]->v + Chassis.pDCMoto[1]->v + Chassis.pDCMoto[0]->v + Chassis.pDCMoto[2]->v) / 4 * CK * 0.13f;
}
/* 获取绝对坐标系下速度 */
#define DK 1.0f
static void Get_XY_Speed_And_Coord(void)
{
	struct Chassis_Data *pData = Chassis.priv_data;
	float theta = Chassis.Z_angle/57.3f; // 改Chassis.Z_angle
	Chassis.VX = (arm_cos_f32(theta) * Chassis.Vx + arm_sin_f32(theta) * Chassis.Vy)*DK;
	Chassis.VY = (-arm_sin_f32(theta) * Chassis.Vx + arm_cos_f32(theta) * Chassis.Vy)*DK;
	
	pData->Current_X += Chassis.VX * 0.005f;
	pData->Current_Y += Chassis.VY * 0.005f;
}
/* hwt101初始化 */
static void hwt101_init(void)
{
	unsigned char Cmd[5] = {0xFF,0xAA,0x76,0x00,0x00};
	while(1)
	{
		if (hwt101.clear_ok)
			break;
		HAL_UART_Transmit_IT(&huart2, Cmd, 5); // ����������
		HAL_Delay(200);
	}
	HAL_UART_Receive_IT(&huart2, &ucTemp, sizeof(ucTemp)); // �����жϷ���
}
/*  状态切换  */
static void Switch_Chassis_State(void)
{
	static uint8_t last_mode = Chassis_Static_Mode;
	
	if (Chassis.ChassisMode != Chassis_Static_Mode && last_mode == Chassis_Static_Mode) // 检测到状态切换
	{
		Chassis.f_StateChange = 1; // 状态切换->开始执行上层任务
		last_mode = Chassis.ChassisMode;
	}
	else if (last_mode != Chassis_Static_Mode && Chassis.ChassisMode == Chassis_Static_Mode && Chassis.f_StateChange)
	{
		Chassis.f_StateChange = 0; // 完成任务
		last_mode = Chassis_Static_Mode;
		osEventFlagsSet(xProcessEventGroupHandle, CHASSIS_DONE); // 任务完成发送事件组标志位置1
	}
}
/* 接收来自上层Process_Task的信息 */ 
#define Chassis_Recv_Flag 0x00001000U
static void Get_Chassis_Ctrl_Data_From_Up(void)
{
	uint32_t Flags = 0;
	Chassis_Rx_Message_t g_Rx_Chassis_Message;
	Flags = osEventFlagsGet(xProcessEventGroupHandle);
	if (Flags & Chassis_Recv_Flag)
	{
		if (osMessageQueueGet(ChassisMessageHandle, &g_Rx_Chassis_Message, NULL, 0) == osOK) {
			osEventFlagsClear(xProcessEventGroupHandle, Chassis_Recv_Flag);// 清零事件组标志位
			Chassis.ChassisMode = g_Rx_Chassis_Message.ChassisMode; // 接收状态
			switch (Chassis.ChassisMode)
			{
			case Chassis_Static_Mode:
				/* code */
				break;
			case Chassis_Move_Mode:
				Chassis.TX = g_Rx_Chassis_Message.Tx;
				Chassis.TY = g_Rx_Chassis_Message.Ty;
				Chassis.Tangle = g_Rx_Chassis_Message.Tangle;
				break;
			case Chassis_Turn_Mode:
				Chassis.TX = g_Rx_Chassis_Message.Tx;
				Chassis.TY = g_Rx_Chassis_Message.Ty;
				Chassis.Tangle = g_Rx_Chassis_Message.Tangle;
				break;
			case Chassis_Linear_Turn_Mode:
				Chassis.TX = g_Rx_Chassis_Message.Tx;
				Chassis.TY = g_Rx_Chassis_Message.Ty;
				Chassis.Tangle = g_Rx_Chassis_Message.Tangle;
				break;		
			case Chassis_Round_Turn_Mode: // 感觉太骚了还是别用了
				break;
			case Chassis_Vision_Location_Mode: // 视觉
				break;
			default:
				break;
			}
		}
	}
}
